/*******************************************************************************

  Robot Toolkit ++ (RTK++)

  Copyright (c) 2007-2014 Shuhui Bu <bushuhui@nwpu.edu.cn>
    http://www.adv-ci.com

  ----------------------------------------------------------------------------

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program. If not, see <http://www.gnu.org/licenses/>.

*******************************************************************************/


#include <math.h>

#include "AHRS_GLViewer.h"

using namespace pi;


// Constructor must call the base class constructor.
AHRS_GLViewer::AHRS_GLViewer(QWidget *parent)
    : QGLViewer(parent)
{
    restoreStateFromFile();

    m_yaw   = 0.0;
    m_pitch = 0.0;
    m_roll  = 0.0;
    m_disp[0] = 0.0;
    m_disp[1] = 0.0;
    m_disp[2] = 0.0;

    this->setBackgroundColor(QColor(0, 0, 0));

    startTimer(30);
}

void AHRS_GLViewer::draw_1()
{
    int     i, j;
    int     v1, v2, v3;
    double  *v, *n;

    glPushMatrix();

    // rotation & translation
    if( 1 ) {
        double  SE3[16];

        //printf("yaw, pitch, roll = %f %f %f\n", m_yaw, m_pitch, m_roll);
        EulerDegToGLMatrix(SE3, m_yaw, m_pitch, m_roll);

        glMatrixMode(GL_MODELVIEW);
        glMultMatrixd(SE3);
    } else {
        float SE3[16];

        m_ang.toGLMatrix(SE3);

        glMatrixMode(GL_MODELVIEW);
        glMultMatrixf(SE3);
    }

    glTranslated(m_disp[0], m_disp[1], m_disp[2]);

    // draw model
    glBegin(GL_TRIANGLES);

    v = m_mesh.vex_arr;
    n = m_mesh.ele_norm;

    for(i=0; i<m_mesh.ele_num; i++) {
        v1 = m_mesh.ele_arr[i*3+0];
        v2 = m_mesh.ele_arr[i*3+1];
        v3 = m_mesh.ele_arr[i*3+2];

        glColor3f(1, 1, 1);
        glNormal3d(n[i*3+0],  n[i*3+1],  n[i*3+2]);

        glVertex3d(v[v1*3+0], v[v1*3+1], v[v1*3+2]);
        glVertex3d(v[v2*3+0], v[v2*3+1], v[v2*3+2]);
        glVertex3d(v[v3*3+0], v[v3*3+1], v[v3*3+2]);
    }

    glEnd();

    glPopMatrix();
}

void AHRS_GLViewer::draw_2()
{
    float   R[9];

    float   ux, vx, wx,
            uy, vy, wy,
            uz, vz, wz;
    float   axis_leng = 100;

    glPushMatrix();

    glTranslated(m_disp[0], m_disp[1], m_disp[2]);

    // convert quaternions to rotation matrix
    m_ang.toRotMat(R);

    for(int i=0; i<9; i++) R[i] = R[i] * axis_leng;

    ux = R[0];  vx = R[3];  wx = R[6];
    uy = R[1];  vy = R[4];  wy = R[7];
    uz = R[2];  vz = R[5];  wz = R[8];

    glLineWidth(5.0);

    glBegin(GL_LINES);

        // draw x-axis
        glColor3f(1, 0, 0);
        glVertex3f(0.0, 0.0, 0.0);
        glVertex3f(ux, vx, wx);

        // draw y-axis
        glColor3f(0, 1, 0);
        glVertex3f(0.0, 0.0, 0.0);
        glVertex3f(uy, vy, wy);

        // draw z-axis
        glColor3f(0, 0, 1);
        glVertex3f(0.0, 0.0, 0.0);
        glVertex3f(uz, vz, wz);

    glEnd();

    glPopMatrix();
}

void AHRS_GLViewer::draw()
{
    // draw airplane model
    draw_1();

    // draw axes
    //draw_2();

    // draw current states
#if 0
    if( 1 ) {
        char  buf[200];
        int   fontSize = 10;
        int   x, y, y_inc;

        y_inc = fontSize*1.5;

        QFont fnt("DejaVu Sans YuanTi Mono", fontSize);

        glColor3f(1.0, 1.0, 1.0);

        x = 10;
        y = y_inc;

        sprintf(buf, "yaw= %12f, pitch = %12f, roll = %12f",
                ahrs_frame.yaw, ahrs_frame.pitch, ahrs_frame.roll);
        this->drawText(x, y, buf, fnt);

        y += y_inc;
        sprintf(buf, "Gx = %12d, Gy    = %12d, Gz   = %12d",
                ahrs_frame.Gx, ahrs_frame.Gy, ahrs_frame.Gz);
        this->drawText(x, y, buf, fnt);

        y += y_inc;
        sprintf(buf, "Ax = %12d, Ay    = %12d, Az   = %12d",
                ahrs_frame.Ax, ahrs_frame.Ay, ahrs_frame.Az);
        this->drawText(x, y, buf, fnt);

        y += y_inc;
        sprintf(buf, "Mx = %12d, My    = %12d, Mz   = %12d",
                ahrs_frame.Mx, ahrs_frame.My, ahrs_frame.Mz);
        this->drawText(x, y, buf, fnt);
    }
#endif
}

void AHRS_GLViewer::timerEvent(QTimerEvent *event)
{
    static uint64_t c = 0;
    if( c++ % 20 == 0 ) saveStateToFile();

    updateGL();
}

void AHRS_GLViewer::keyPressEvent(QKeyEvent *e)
{
    // Get event modifiers key
    const Qt::KeyboardModifiers modifiers = e->modifiers();

    // A simple switch on e->key() is not sufficient if we want to take state key into account.
    // With a switch, it would have been impossible to separate 'F' from 'CTRL+F'.
    // That's why we use imbricated if...else and a "handled" boolean.

    if ((e->key()==Qt::Key_W) && (modifiers==Qt::NoButton)) {
        this->camera()->setPosition(qglviewer::Vec(0, 0, 1));
        this->camera()->setViewDirection(qglviewer::Vec(0, 0, -1));
        this->camera()->setUpVector(qglviewer::Vec(0, 1, 0));
        showEntireScene();

        updateGL();
    }
    else if ( e->key() == Qt::Key_Escape ) {

    }
    else if ( (e->key() == Qt::Key_R) && (modifiers==Qt::NoButton) ) {
        showEntireScene();
    }
    else {
        QGLViewer::keyPressEvent(e);
    }
}

void AHRS_GLViewer::closeEvent(QCloseEvent *e)
{
    saveStateToFile();
}

QString AHRS_GLViewer::helpString() const
{
    QString text("<h2>AHRS OpenGL Viewer</h2>");
    return text;
}

int AHRS_GLViewer::load_mesh(std::string &fn_mesh)
{
    int ret;

    ret = m_mesh.load(fn_mesh.c_str());
    m_mesh.mesh_normalize();

    double br = m_mesh.mesh_bound_radius();
    this->setSceneRadius(br);

    return ret;
}
